import os
import sys

sys.path.append(".")

import math
import numpy as np
# import converter
import matplotlib
matplotlib.use('TkAgg')
from matplotlib import pyplot as plt
from matplotlib.lines import Line2D
from mpl_toolkits.mplot3d import Axes3D
from mpl_toolkits.mplot3d.art3d import Line3D
import time_processor as tp
import geo_processor as gp
from scipy import interpolate

def read_gps_data(gps_path):

    result = []

    num = 0
    with open(gps_path) as f:
        for line in f:
            if line[0] != '#':
                pass
            num = num + 1 
            items = line.split(' ')
    
            result1 = []
            result1.append(int(items[0])) # timestamp
            result1.append(float(items[1])) # lat
            result1.append(float(items[2])) # lon
            result1.append(float(items[3])) # hei

            result.append(result1)

    return np.array(result)

def show_comp_gps_data(data1, data2):

    gps_org = [30.595592610010, 114.238722681970,23.715600000000]

    max_ts_1 = np.max(data1[:,0])
    min_ts_1 = np.min(data1[:,0])

    data2_index1 = np.where(data2[:,0] < max_ts_1)
    data2_index2 = np.where(data2[:,0] > min_ts_1)

    data2_temp = data2[data2_index1[0],:]
    
    fx = interpolate.interp1d(data1[:,0], data1[:,1], kind = "quadratic")
    fy = interpolate.interp1d(data1[:,0], data1[:,2], kind = "quadratic")
    fz = interpolate.interp1d(data1[:,0], data1[:,3], kind = "quadratic")
    xnew = fx(data2_temp[:,0])
    ynew = fy(data2_temp[:,0])
    znew = fz(data2_temp[:,0])

    enu_new = gp.geodetic_to_enu_series(xnew, ynew, znew, gps_org[0], gps_org[1], gps_org[2])
    enu_old = gp.geodetic_to_enu_series(data2_temp[:,1], data2_temp[:,2], data2_temp[:,3], gps_org[0], gps_org[1], gps_org[2])
    
    fig = plt.figure()
    ax = Axes3D(fig)
    ax.scatter(enu_new[:,0], enu_new[:,1], enu_new[:,2])
    ax.scatter(enu_old[:,0], enu_old[:,1], enu_old[:,2])
    for i in range(enu_new.shape[0]):
        if i % 200 != 0:
            continue
        line1 = [(enu_new[i,0], enu_new[i,1], enu_new[i,2]), (enu_old[i,0], enu_old[i,1], enu_old[i,2])]
        (line1_xs, line1_ys, line1_zs) = zip(*line1)
        ax.add_line(Line3D(line1_xs, line1_ys, line1_zs, linewidth=1, color='blue'))


    # ax1 = plt.subplot(111)
    # ax1.scatter(enu_new[:,0], enu_new[:,1], s=2, c='y')
    # ax1.scatter(enu_old[:,0], enu_old[:,1], s=2, c='r')
    # for i in range(enu_new.shape[0]):
    #     if i % 200 != 0:
    #         continue
    #     line1 = [(enu_new[i,0], enu_new[i,1]), (enu_old[i,0], enu_old[i,1])]
    #     (line1_xs, line1_ys) = zip(*line1)
    #     ax1.add_line(Line2D(line1_xs, line1_ys, linewidth=1, color='blue'))
    plt.show()

def get_gap_time(data):
    gap_t = []
    for i in range(data[:,0].shape[0]-1):
        if data[i+1,0] - data[i,0] > 1000000000:
            gap_t.append(data[i,0])
            gap_t.append(data[i+1,0])
    return gap_t

if __name__ == "__main__":
    gps_path1 = r"/shared/20210902/data/data/ins.txt"
    gps_path2 = r"/media/cjg/Elements/pano20210902/pro2/data/gps_new.txt"
    gps_result1 = read_gps_data(gps_path1)
    gps_result2 = read_gps_data(gps_path2)
    # show_comp_gps_data(gps_result1, gps_result2)

    print(get_gap_time(gps_result1))
